#include "UltraWave.h"

void UltraWaveInit(void)
{
    P3DIR |= BIT5;
    P3DIR &= ~BIT6;
    P3REN |= BIT6; // pull up
    P3OUT |= BIT6;
    P3OUT &= ~BIT5;
}

long int GetDistance(void)
{
    //P3.5->TRIG
    //p3.6->ECHO
    int timeOut = 0;
    P3OUT &= ~BIT5;
    P3OUT |= BIT5;
    delayUs(11);
    P3OUT &= ~BIT5;
    long int us = 0;
    timeOut = 0;

    while((P3IN & BIT6) == 0 && timeOut++ < 1000) { // check LOW
        delayUs(1);
    }
    if (timeOut >= 1000) {
        UART_Puts("time out1\r\n");
        return 0;
    }
    timeOut = 0;

    while((P3IN & BIT6) != 0 && timeOut++ < 2000) { // check HIGH
        us++;
    }
    if (timeOut >= 2000) {
        UART_Puts("time out2\r\n");
        return 0;
    }
    UART_PutNum(us, 1);
    return us;
}

